#include <assert.h>
#include "log4cplus\logger.h"
#include "Vector.h"
#include "point.h"
#include "normal.h"
#include "matrix.h"
#include "transform.h"

using namespace log4cplus;
using namespace vivid;

Transform::Transform()
{
	_m = Matrix::Identity();
	_mInv = _m;
}


Transform::Transform(Transform const & aT)
{
	_m = aT._m;
	_mInv = aT._mInv;
}


Transform::Transform(Matrix const & aM)
{
	_m = aM;
	_mInv = _m.GetInverse();
}


Transform::Transform(Matrix const &aM, Matrix const & aInv)
{
    _m = aM;
    _mInv = aInv;
}


bool Transform::operator == (Transform const &aT) const
{
	return _m == aT._m;
}


bool Transform::operator != (Transform const &aT) const
{
	return _m != aT._m;
}


Transform Transform::operator *(Transform const & aT) const
{
    return Transform(_m * aT._m, _mInv * aT._mInv);
}


Point Transform::operator *(Point const &aP) const
{
    VFloat wx = _m[0][0] * aP._x + _m[0][1] * aP._y + _m[0][2] * aP._z + _m[0][3];
    VFloat wy = _m[1][0] * aP._x + _m[1][1] * aP._y + _m[1][2] * aP._z + _m[1][3];
    VFloat wz = _m[2][0] * aP._x + _m[2][1] * aP._y + _m[2][2] * aP._z + _m[2][3];
    VFloat wp = _m[3][0] * aP._x + _m[3][1] * aP._y + _m[3][2] * aP._z + _m[3][3];
    assert(wp != 0);

    if (wp == 1.0)
    {
        return Point(wx, wy, wz);
    }

    return Point(wx / wp, wy / wp, wz / wp);
}


Vector Transform::operator *(Vector const &aV) const
{
    return Vector(
        _m[0][0] * aV._x + _m[0][1] * aV._y + _m[0][2] * aV._z,
        _m[1][0] * aV._x + _m[1][1] * aV._y + _m[1][2] * aV._z,
        _m[2][0] * aV._x + _m[2][1] * aV._y + _m[2][2] * aV._z);
}


Normal Transform::operator *(Normal const &aN) const
{
    return Normal(
        _mInv[0][0] * aN._x + _mInv[0][1] * aN._y + _mInv[0][2] * aN._z,
        _mInv[1][0] * aN._x + _mInv[1][1] * aN._y + _mInv[1][2] * aN._z,
        _mInv[2][0] * aN._x + _mInv[2][1] * aN._y + _mInv[2][2] * aN._z);
}


Transform Transform::GetInverse() const
{
    return Transform(_mInv, _m);
}


Transform Transform::GetTranspose() const
{
    return Transform(_m.GetTranspose(), _mInv.GetTranspose());
}


static Transform Translate(VFloat aX, VFloat aY, VFloat aZ)
{
    return 
        Transform(
            Matrix( 1, 0, 0, aX,
                    0, 1, 0, aY,
                    0, 0, 1, aZ,
                    0, 0, 0, 1),
            Matrix( 1, 0, 0, -aX,
                    0, 1, 0, -aY,
                    0, 0, 1, -aZ,
                    0, 0, 0, 1));
}

static Transform Scale(VFloat aX, VFloat aY, VFloat aZ)
{
    return Transform(
            Matrix( aX, 0,  0, 0,
                    0, aY,  0, 0,
                    0,  0, aZ, 0,
                    0,  0,  0, 1),
            Matrix( 1.0/aX, 0,  0, 0,
                    0, 1.0/aY,  0, 0,
                    0,  0, 1.0/aZ, 0,
                    0,  0,  0, 1));

}


static Transform RotateX(VFloat aAng)
{
    VFloat sinT = sin(Rad(aAng));
    VFloat cosT = cos(Rad(aAng));

    Matrix m( 1, 0, 0, 0,
              0, cosT, -sinT, 0,
              0, sinT, cosT, 0,
              0, 0, 0, 1);

    return Transform(m, m.GetTranspose());
}


static Transform RotateY(VFloat aAng)
{
    VFloat sinT = sin(Rad(aAng));
    VFloat cosT = cos(Rad(aAng));

    Matrix m( cosT, 0, sinT, 0,
              0, 1, 0, 0,
              -sinT, 0, cosT, 0,
              0, 0, 0, 1);

    return Transform(m, m.GetTranspose());
}


static Transform RotateZ(VFloat aAng)
{
    VFloat sinT = sin(Rad(aAng));
    VFloat cosT = cos(Rad(aAng));

    Matrix m( cosT, -sinT, 0, 0,
              sinT, cosT, 0, 0,
              0, 0, 0, 0,
              0, 0, 0, 1);

    return Transform(m, m.GetTranspose());
}


static Transform Rotate(VFloat aAng, Vector const & aAxis)
{
    Vector a = aAxis.Normalize();
    VFloat s = sin(Rad(aAng));
    VFloat c = cos(Rad(aAng));

    Matrix m(
        a._x * a._x + (1.f - a._x * a._x) * c,
        a._x * a._y * (1.f - c) - a._z * s,
        a._x * a._z * (1.f - c) + a._y * s,
        0,
        a._x * a._y * (1.f - c) + a._z * s,
        a._y * a._y + (1.f - a._y * a._y) * c,
        a._y * a._z * (1.f - c) - a._x * s,
        0,
        a._x * a._z * (1.f - c) - a._y * s,
        a._y * a._z * (1.f - c) + a._x * s,
        a._z * a._z + (1.f - a._z * a._z) * c,
        0, 0, 0, 0, 1);

    return Transform(m, m.GetTranspose());
}


static Transform LookAt(Point const & aPos, Point const & aLook, Vector const & aUp)
{
    // Initialize first three columns of viewing matrix
    Vector dir = (aLook - aPos).Normalize();
    Vector left = aUp.Normalize().Cross(dir);
    Vector newUp = dir.Cross(left);

    Matrix m(
        left._x, newUp._x, dir._x, aPos._x,
        left._y, newUp._y, dir._y, aPos._y,
        left._z, newUp._z, dir._z, aPos._z,
        0,       0,        0,      1);
 
    return Transform(m.GetInverse(), m);
}

